#include "JacbiCor.h"

/**
* @brief 求实对称矩阵的特征值及特征向量的雅克比法 
* @param pMatrix				长度为n*n的数组，存放实对称矩阵
* @param n  					矩阵的阶数 
* @param pdblVects				长度为n*n的数组，返回特征向量(按列存储) 
* @param pdbEigenValues			特征值数组
* @param dbEps					精度要求 
* @param nJt					整型变量，控制最大迭代次数 
*/
void JacbiCor(double * pMatrix,int n, double *pdblVects, double *pdbEigenValues, double dbEps,int nJt)
{ for(int i = 0; i < n; i++) 
  { pdblVects[i*n+i] = 1.0f; 
    for(int j = 0; j < n; j ++) 
    { if(i != j)   
        pdblVects[i*n+j]=0.0f; 
    } 
  } 
 
  int nCount = 0;        //迭代次数
  while(1)
  { //在pMatrix的非对角线上找到最大元素
    double dbMax = pMatrix[1];
    int nRow = 0;
    int nCol = 1;
    for (int i = 0; i < n; i ++)            //行
    { for (int j = 0; j < n; j ++)          //列
      { double d = fabs(pMatrix[i*n+j]); 
        if((i!=j) && (d> dbMax)) 
        { dbMax = d;   
          nRow = i;   
          nCol = j; 
        } 
      }
    }
 
    if(dbMax < dbEps)      //精度符合要求 
      break;  
 
    if(nCount > nJt)       //迭代次数超过限制
      break;
 
    nCount++;
 
    double dbApp = pMatrix[nRow*n + nRow];
    double dbApq = pMatrix[nRow*n + nCol];
    double dbAqq = pMatrix[nCol*n + nCol];

    //计算旋转角度
    double dbAngle = 0.5*atan2(-2*dbApq, dbAqq-dbApp);
    double dbSinTheta = sin(dbAngle);
    double dbCosTheta = cos(dbAngle);
    double dbSin2Theta = sin(2*dbAngle);
    double dbCos2Theta = cos(2*dbAngle);
 
    pMatrix[nRow*n+nRow] = dbApp*dbCosTheta*dbCosTheta + dbAqq*dbSinTheta*dbSinTheta + 2*dbApq*dbCosTheta*dbSinTheta;
    pMatrix[nCol*n+nCol] = dbApp*dbSinTheta*dbSinTheta + dbAqq*dbCosTheta*dbCosTheta - 2*dbApq*dbCosTheta*dbSinTheta;
    pMatrix[nRow*n+nCol] = 0.5*(dbAqq-dbApp)*dbSin2Theta + dbApq*dbCos2Theta;
	pMatrix[nCol*n+nRow] = pMatrix[nRow*n + nCol];
 
    for(int i = 0; i < n; i ++) 
    { if((i!=nCol) && (i!=nRow)) 
      { int u = i*n + nRow;    //p  
        int w = i*n + nCol;    //q
        dbMax = pMatrix[u]; 
        pMatrix[u]= pMatrix[w]*dbSinTheta + dbMax*dbCosTheta; 
        pMatrix[w]= pMatrix[w]*dbCosTheta - dbMax*dbSinTheta; 
      } 
    } 
 
    for (int j = 0; j < n; j ++)
    { if((j!=nCol) && (j!=nRow)) 
      { int u = nRow*n + j;    //p
        int w = nCol*n + j;    //q
        dbMax = pMatrix[u]; 
        pMatrix[u]= pMatrix[w]*dbSinTheta + dbMax*dbCosTheta; 
        pMatrix[w]= pMatrix[w]*dbCosTheta - dbMax*dbSinTheta; 
      } 
    }
 
    //计算特征向量
    for(int i = 0; i < n; i ++) 
    { int u = i*n + nRow;        //p   
      int w = i*n + nCol;        //q
      dbMax = pdblVects[u]; 
      pdblVects[u] = pdblVects[w]*dbSinTheta + dbMax*dbCosTheta; 
      pdblVects[w] = pdblVects[w]*dbCosTheta - dbMax*dbSinTheta; 
    } 
  }

  //取特征值
  for(int i = 0; i < n; i ++) 
    pdbEigenValues[i] = pMatrix[i*n+i];

  return;
}

